Accounting for Parametric Model Uncertainty in Collision Avoidance for Unmanned Vehicles Using Sparse Grid Interpolation

In this paper we present an enhancement of model-based trajectory selection algorithms – a popular class of collision avoidance techniques for autonomous ground vehicles. Rather than dilate a set of individual candidate trajectories in an ad hoc way to account for uncertainty, we generate a set of trajectory clouds – sets of states that represent possible future poses over a product of intervals representing uncertainty in the model parameters, initial conditions and actuator commands. The clouds are generated using the sparse-grid interpolation method which is both error-controlled and computationally efficient. The approach is implemented on a differential drive vehicle. INTRODUCTION Many autonomous ground vehicles (AGVs) employ the multi-layer motion planning framework illustrated in Fig. 1. The high-level planner (a.k.a. mission or route planner) generates a series of way points, or sub-goals, using an a priori map reflecting the location of fixed obstacles, the presence and directionality of roadways and other mission constraints. A collision avoidance algorithm (a.k.a. trajectory generator or low-level planner), then generates admissible steering inputs to progress toward this way point, while avoiding collisions or undrivable terrain. At the lowest level, these steering commands are usually then implemented by a PID-based steering controller. Nearly every entry in the DARPA Grand [1] and Urban Challenges [2] and European Land Robotics Trials employed this multi-level planning paradigm. In this paper we focus on a particular class of collision avoidance algorithms referred to here as model-based trajectory selection algorithms. Such approaches rely on a kinematic or dynamic model of the vehicle to evaluate a set of candidate steering inputs, based on the predicted trajectory of the vehicle. One major limitation of model-based approaches is that the vehicle's dynamics can be quite difficult to model. So called "first principle models" often are only an approximation to higher order phenomena. Even if the model's structure is accurate, estimating the underlying parameters can be quite difficult. For example, the rolling resistance coefficient is not constant, but rather a function of the terrain, tire tread wear, and temperature. As a result, nearly all model-based trajectory selection algorithms introduce a safety factor by dilating the predicted trajectories, prior to evaluation, in order to account for uncertainties in the future position of the vehicle. The dilation radius is frequently chosen in an ad hoc or heuristic fashion. In this paper we present an enhancement of model-based trajectory selection algorithms which provides a principled and computationally efficient way to account for parametric uncertainty in the vehicle's dynamics using an error-controlled sparse grid interpolant. In this approach, the vehicle dynamics are simulated at model parameter values which are sparsely sampled from an estimated range of uncertainty. The simulations support the construction of a polynomial-based interpolant that describes the potential future trajectories of the vehicle subject to the simulated uncertainty. The remainder of this paper is organized as follows. The following section presents a detailed review of the algorithmic structure of model-based trajectory planners, as well as approaches for dealing with parameter uncertainty. The Method section describes how sparse grid interpolation techniques can be used to enhance such planners by accounting for this uncertainty in a principled and computationally efficient fashion by generating trajectory clouds. The Model section describes the differential drive vehicle model used to illustrate the approach. The Implementation section describes how the collision avoidance algorithm was modified and Navigation Results provides sample results. Finally, the Discussion section presents the advantages of this approach. FIGURE 1. A COMMON MULTI-LEVEL MOTION PLANNING FRAMEWORK FOR AUTONOMOUS VEHICLES LITERATURE REVIEW There are many schemes for collision avoidance including variants on the potential field method [3], the dynamic window approach [4], vector field histograms [5], and graph search algorithms like D* [6]. However, these algorithms generate a desired velocity or input force which may not be directly implementable on a real vehicle. Non-holonomic kinematics, turning radius constraints, momentum, and actuator limits preclude certain instantaneous velocities. In the case of graph search algorithms the resulting trajectories can be also jagged and inefficient. Such algorithms require some type of post processing to produce feasible steering commands. In doing so, there is a risk that the collision avoidance guarantees of the original algorithm will be lost. A very popular alternate approach is to collision check a local occupancy grid (a.k.a. ego-grid or cost-map) against a set of pre-computed trajectories generated directly from a discreet set of feasible inputs. A generic framework for such approaches is provided in Fig. 2 and illustrated in Fig. 3. Early implementations include [7] and [8], while more recent variants include [9] and [10]. They have been implemented extensively on ground vehicles in events such as DARPA Grand and Urban Challenges, and the European Land Robotics Trials (see for example [11-18]). More formally this is related to some of the Model Based Control concepts introduced in [11] and [12]. For each step in the algorithm shown in Fig. 2 there are many variations in the literature; however we focus on the precomputation steps 2 and 3 – the generation of the candidate trajectories and their dilation in such a way as to account for uncertainty. Dilation of the trajectory by a constant, albeit speed dependent, width is typical [7,8]. A more sophisticated approach would be to construct a probabilistic motion model such as that proposed in [20] Chapt. 5. However, the model used there affords a closed form solution and has only 4 uncertain parameters, making it difficult to extend to arbitrary motion models in a computationally efficient fashion. A general treatment of motion planning with model and sensing uncertainty requires the frame work of partially observable Markov decision processes (POMDPs), which is known to be intractable [21]. Approximations can render the approach viable in special cases, but simulating candidate trajectories is still required, similar to Fig. 2 (Precomputation Step 2 and 3). FIGURE 2. A GENERIC OUTLINE FOR MODEL-BASED TRAJECTORY SELECTION ALGORITHMS FIGURE 3. AN ILLUSTRATION OF MODEL-BASED TRAJECTORY SELECTION ALGORITHMS WITH AD HOC TRAJECTORY DILATION obstacle Algorithm: Model-based Trajectory Selection Pre-computation 1. Create a discretized representation of the set of admissible inputs and state variables. 2. Working in a body-fixed frame, generate trajectory segments corresponding to each of the inputs, at each of the states, from step 1. 3. Dilate these segments as needed to account for the size of the vehicle and any additional clearance required to account for uncertainty. 4. Set the resolution of the body fixed occupancy grid. Compute a lookup table indicating which cells support a given trajectory, include possible weighting coefficients to be used in path selection. while (1) 1. Determine desired direction of travel using a global planner 2. Acquire sensor data describing surrounding terrain 3. Populate body-fixed occupancy grid 4. Evaluate each candidate trajectory segment for potential of collision or other fitness criteria (e.g. deviation from desired heading and speed) 5. Execute steering inputs corresponding to the most desirable trajectory from Step 4, via the steering controller end High Level Planner Collision Avoidance Algorithm Steering Controller Estimated Pose Goal Pose A Priori Map Way Point Steering Inputs Terrain Sensor Data METHOD Consider Fig. 2: Precomputation Steps 2 and 3. Typically a candidate trajectory is computed based on the nominal vehicle dynamics as follows. Assume a vehicle has a dynamic model with states N x R ∈ , outputs Q y R ∈ , inputs M u U R ∈ ⊂ , and model parameters D p P R ∈ ⊂ . A state (or output) transition equation ( , , , ) k o y t u x p , which may be closed-form or evaluated via numerical simulation, can be used to predict the future output at time k t R + ∈ , assuming the initial state was 0 0 ( ) x x t = . Then an arbitrary safety margin is applied by taking the Minkowski sum of the outputs with a ball of radius r along a short but dense interval of times 0 1 [ , , , ] K T t t t =  (usually determined by the time steps of